import os
import cv2
import numpy as np

def process_bias(binary_image, original_image):
    # Apply adaptive thresholding for better road detection
    adaptive_thresh = cv2.adaptiveThreshold(
        original_image,
        255,
        cv2.ADAPTIVE_THRESH_GAUSSIAN_C,
        cv2.THRESH_BINARY_INV,
        11,
        2
    )
    
    # Apply morphological operations to clean up the image
    kernel = np.ones((5, 5), np.uint8)
    cleaned_image = cv2.morphologyEx(adaptive_thresh, cv2.MORPH_CLOSE, kernel)
    cleaned_image = cv2.morphologyEx(cleaned_image, cv2.MORPH_OPEN, kernel)
    
    height = cleaned_image.shape[0]
    width = cleaned_image.shape[1]
    
    # Calculate image midpoint
    mid_x = width // 2
    
    # Get all black pixel coordinates
    black_pixels = np.where(cleaned_image > 0)
    y_coords = black_pixels[0]
    x_coords = black_pixels[1]
    
    if len(x_coords) > 0:
        # Create points array for cv2.convexHull
        points = np.column_stack((x_coords, y_coords))
        hull = cv2.convexHull(points)
        
        # Extract x and y coordinates from hull points
        hull_points = hull.reshape(-1, 2)
        hull_x = hull_points[:, 0]
        hull_y = hull_points[:, 1]
        
        # Separate hull points into left and right based on median x-coordinate
        median_x = np.median(hull_x)
        left_mask = hull_x < median_x
        right_mask = hull_x >= median_x
        
        # Get left and right points
        left_x = hull_x[left_mask]
        left_y = hull_y[left_mask]
        right_x = hull_x[right_mask]
        right_y = hull_y[right_mask]
        
        # Fit lines
        left_fit = np.polyfit(left_y, left_x, 1)
        right_fit = np.polyfit(right_y, right_x, 1)
        
        # Calculate midline at the middle height of the image
        mid_height = height // 2
        left_x_at_mid = np.polyval(left_fit, mid_height)
        right_x_at_mid = np.polyval(right_fit, mid_height)
        midline_at_center = (left_x_at_mid + right_x_at_mid) / 2
        
        # Calculate normalized bias (0 to 100)
        bias = 50 * (1 - (midline_at_center - mid_x) / mid_x)
        bias = np.clip(bias, 0, 100)
        
        return bias
    else:
        print("No black pixels detected")
        return None

def get_bias(input_path):
    if os.path.isfile(input_path):
        # Read image and convert to grayscale
        original_image = cv2.imread(input_path, cv2.IMREAD_GRAYSCALE)
        
        if original_image is None:
            print(f"Error: Could not read image at {input_path}")
            return
        
        # Create binary image
        _, binary_image = cv2.threshold(original_image, 120, 255, cv2.THRESH_BINARY)
        
        # Calculate bias
        bias = process_bias(binary_image, original_image)
        return bias
    else:
        print(f"Error: File not found at {input_path}")

if __name__ == '__main__':
    input_path = 'test_image.png'  # Update this path as needed
    bias = get_bias(input_path)
    if bias is not None:
        print(f"Bias: {bias:.1f}")

